
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <vector>
#include <string>

using namespace std;

typedef struct
{
	int num_squares_height;
	int num_squares_width;
	vector<string> images;
}check_board_detection_data;

check_board_detection_data data;

int num_images = 0;
int num_boards_detected = 0;


void
show_usage_information_and_exit(int argc, char *argv[])
{
	printf("\n");
	printf("Use %s <num_squares_height> <num_squares_width> <image_list>\n", argv[0]);
	printf("\n");

	exit(-1);
}


void
read_image_list(char *filename)
{
	char image_filename[1024];

	FILE *fptr = fopen(filename, "r");

	while(!feof(fptr))
	{
		fscanf(fptr, "\n%[^\n]\n", image_filename);
		data.images.push_back(string(image_filename));
	}

	fclose(fptr);
}


int
parse_arguments(int argc, char *argv[])
{
	if (argc < 3)
		return 0;

	data.num_squares_height = atoi(argv[1]);
	data.num_squares_width = atoi(argv[2]);

	read_image_list(argv[3]);

	return 1;
}


void
detect_and_print_corners(char *image_filename)
{
	int board_w = data.num_squares_width;
	int board_h = data.num_squares_height;

	int board_n = board_w * board_h;

	CvSize board_sz = cvSize(board_w, board_h);

	CvMat *image_points = cvCreateMat(board_n, 2, CV_32FC1);
	CvMat *object_points= cvCreateMat(board_n, 3, CV_32FC1);
	CvMat *point_counts = cvCreateMat(1, 1, CV_32SC1);

	CvPoint2D32f *corners = new CvPoint2D32f[board_n];

	int corner_count;

	IplImage *image = cvLoadImage(image_filename, CV_LOAD_IMAGE_COLOR);

	if (image == NULL)
		exit(printf("Image \"%s\" could not be open!\n", image_filename));

	IplImage *gray_image = cvCreateImage(cvGetSize(image), 8, 1);

	int found = cvFindChessboardCorners(image, board_sz, corners,
			&corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);

	// Get subpixel accuracy on those corners
	cvCvtColor(image, gray_image, CV_BGR2GRAY);

	cvFindCornerSubPix(gray_image, corners, corner_count, cvSize(11, 11),
		cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

	if (found)
	{
		printf("%s", image_filename);

		for(int i = 0; i < board_n; i++)
			printf(" %f %f", corners[i].x, corners[i].y);

		printf("\n");

		num_boards_detected++;
	}
	else
		fprintf(stderr, " (error) ");

	cvReleaseMat(&object_points);
	cvReleaseMat(&image_points);
	cvReleaseMat(&point_counts);

	cvReleaseImage(&image);
	cvReleaseImage(&gray_image);
}


int
main(int argc, char* argv[])
{
	if (!parse_arguments(argc, argv))
		show_usage_information_and_exit(argc, argv);

	num_images = data.images.size();

	for(int i = 0; i < (int) data.images.size(); i++)
	{
		fprintf(stderr, "%d ", i);
		detect_and_print_corners((char*) data.images[i].c_str());
	}

	fprintf(stderr, "\n\n%lf percents of boards detected\n", 100 * (double) num_boards_detected / (double) num_images);
	return 0;
}
